"""
滤波器模块
包含一阶互补滤波器实现
"""

import time

K1 = 0.15
angle_R = 0.0
angle_P = 0.0
rt = pt = time.time()
dt = 0.01


def filter_r(angle_m, gyro_m):
    """
    filter_r:横滚的一阶互补滤波
    angle_m:加速度计得到的角度,这里是x轴的角度
    gyro_m:陀螺仪得到的角速度。这里是y轴陀螺仪值
    """
    global angle_R
    global K1
    global dt
    angle_R = K1 * angle_m + (1 - K1) * (angle_R + gyro_m * dt)

    return angle_R


def filter_p(angle_m, gyro_m):
    """
    filter_p:俯仰的一阶互补滤波
    angle_m:加速度计得到的角度,这里是y轴的角度
    gyro_m:陀螺仪得到的角速度。这里是x轴陀螺仪值
    """
    global angle_P
    global K1
    global dt
    angle_P = K1 * angle_m + (1 - K1) * (angle_P + gyro_m * dt)

    return angle_P
